#include "AutoPilot.h"
using namespace alglib;

AP_AnalyseScan::AP_AnalyseScan() {
	if (AP_DEBUG) cout << "AP_AnalyseScan::AP_AnalyseScan" << endl;
	dtms = 1000;// initialisation
	pylones.clear();
}

AP_AnalyseScan::~AP_AnalyseScan() {
	if (AP_DEBUG) cout << "AP_AnalyseScan::~AP_AnalyseScan" << endl;
	pylones.clear();
	//normalement TypeScan.tabScan n'est pas un pointeur, allocation statique.
}

inline float AP_AnalyseScan::distTps(TypePointScan a, TypePointScan b) {// calcul approximatif de distance entre deux TypePointScan
	float rad, dr;
	if (AP_DEBUG) cout << "inline float AP_AnalyseScan::distTps" << endl;
	dr = a.distance - b.distance;
	dr = (dr > 0) ? dr : -dr;
	rad = (a.angle - b.angle) * M_PI / 180;
	rad *= (a.distance + b.distance) / 2;
	rad = (rad > 0) ? rad : -rad;
	//return sqrt(dr * dr + rad * rad);
	return dr + rad; // FIXME: Calcul rapide certes, mais trèèès imprécis... peut mieux faire...
}

/*
 * scancpy: recopie _scan dans scan, met à jour dtms
 */
void AP_AnalyseScan::scancpy(const TypeScan _scan) {
	if (AP_DEBUG) cout << "void AP_AnalyseScan::scancpy" << endl;
	for (int i = 0; i < _scan.nbPoints; i++) {
		scan.tabScan[i].distance = _scan.tabScan[i].distance;
		scan.tabScan[i].angle = _scan.tabScan[i].angle;
	}
	dtms = _scan.timeStamp - scan.timeStamp;
	scan.timeStamp = _scan.timeStamp;
	scan.nbPoints = _scan.nbPoints;
}

/*
 * filtrerScan: en fonction du seuil passé en argument, traite le scan passé par référence
 */
void AP_AnalyseScan::filtrerScan(TypeScan& scan_, float seuil) {
	int ref, f1, f2;
	if (AP_DEBUG) cout << "void AP_AnalyseScan::filtrerScan" << endl;
	for (int i = 1; i < scan_.nbPoints - 1; i++) {
		ref = scan_.tabScan[i].distance;
		f1 = scan_.tabScan[i + 1].distance - ref;
		f2 = scan_.tabScan[i - 1].distance - ref;
		f1 = (f1 < 0) ? -f1 : f1;
		f2 = (f2 < 0) ? -f2 : f2;
		if (f1 > seuil && f2 > seuil) scan_.tabScan[i].distance
				= (scan_.tabScan[i + 1].distance
						+ scan_.tabScan[i - 1].distance) / 2;
	}
}

/*
 * interpreter: renvoie dans le vecteur pylones_ passé en argument la liste des centres de poteaux dans le repère du drone
 */
void AP_AnalyseScan::interpreter(
		const TypeScan& _scan,
		float cos_pitch,
		float cos_roll,
		float diametre,
		float rcontig,
		vector<GPoint2D>& pylones_) {
	int i, j, k;
	float taille;
	if (AP_DEBUG) cout << "void AP_AnalyseScan::interpreter" << endl;
	pylones_.clear();
	for (i = 0; i < _scan.nbPoints; i++) {
		j = i;
		// détermination de la taille de l'obstacle et de l'indice de distance minimale
		while (j + 1 < _scan.nbPoints && distTps(
				_scan.tabScan[j + 1],
				_scan.tabScan[j]) < rcontig)
			j++;
		// si l'obstacle a la bonne taille on l'interprète comme un pylone
		taille = distTps(_scan.tabScan[i], _scan.tabScan[j]);
		k = (i + j) / 2;
		if ((taille > .2 * diametre) && (taille < 2 * diametre)) {
			float cos_curangle, sin_curangle, r_pyl, cur_angle, dist;
			cur_angle = scan.tabScan[k].angle * M_PI / 180;
			dist = scan.tabScan[k].distance;
			cos_curangle = cos(cur_angle);
			sin_curangle = sin(cur_angle);
			r_pyl = diametre / 2;
			pylones_.push_back(GPoint2D((dist + r_pyl / cos_pitch)
					* cos_curangle, -(dist + r_pyl / cos_roll) * sin_curangle));
		}
		i = j + 1;
	}
}

inline void AP_AnalyseScan::aplatir(
		vector<GPoint2D>& _pylones,
		float cos_pitch,
		float sin_pitch,
		float cos_roll,
		float sin_roll) {
	if (AP_DEBUG) cout << "inline void AP_AnalyseScan::aplatir" << endl;
	for (unsigned short int i = 0; i < _pylones.size(); i++) {
		// ici chaque pylone est remis à plat
		_pylones[i] = GPoint2D(cos_pitch * _pylones[i].x + sin_pitch * sin_roll
				* _pylones[i].y, cos_roll * _pylones[i].y);
	}
}

inline bool AP_AnalyseScan::fautilanalyser(
		const TypeScan& _scan,
		int nbpointsnominal) {
	if (AP_DEBUG) cout << "inline bool AP_AnalyseScan::fautilanalyser" << endl;
	return _scan.nbPoints == nbpointsnominal && _scan.timeStamp
			!= scan.timeStamp;
}

bool AP_AnalyseScan::analyserScan(
		const TypeScan& _scan,
		float seuil,
		int nbpointsnominal,
		float phi,
		float theta,
		float diametre,
		float rcontig,
		vector<GPoint2D>& pylones_) {
	bool rep = fautilanalyser(_scan, nbpointsnominal);
	if (AP_DEBUG) cout << "bool AP_AnalyseScan::analyserScan" << endl;
	if (rep) {
		float cos_pitch, cos_roll, sin_pitch, sin_roll;
		pylones_.clear();
		cos_pitch = cos(theta);
		sin_pitch = sin(theta);
		cos_roll = cos(phi);
		sin_roll = sin(phi);
		scancpy(_scan);
		filtrerScan(scan, seuil);
		interpreter(scan, cos_pitch, cos_roll, diametre, rcontig, pylones_);
		aplatir(pylones_, cos_pitch, sin_pitch, cos_roll, sin_roll);
	}
	return rep;
}

inline bool AP_AnalyseScan::analyserScan(
		const TypeScan& _scan,
		float seuil,
		int nbpointsnominal,
		float phi,
		float theta,
		float diametre,
		float rcontig) {
	if (AP_DEBUG) cout << "inline bool AP_AnalyseScan::analyserScan" << endl;
	return analyserScan(
			_scan,
			seuil,
			nbpointsnominal,
			phi,
			theta,
			diametre,
			rcontig,
			pylones);
}

AP_ReconRepere::AP_ReconRepere() {
	if (AP_DEBUG) cout << "AP_ReconRepere::AP_ReconRepere" << endl;
	reperes.clear();
	ident.clear();
	pylid.clear();
	repid.clear();
}
AP_ReconRepere::~AP_ReconRepere() {
	if (AP_DEBUG) cout << "AP_ReconRepere::~AP_ReconRepere() " << endl;
	reperes.clear();
	ident.clear();
	pylid.clear();
	repid.clear();
}

void AP_ReconRepere::ajouterRepere(GPoint2D repere) {
	if (AP_DEBUG) cout << "void AP_ReconRepere::ajouterRepere" << endl;
	reperes.push_back(repere);
	ident.push_back(false);
}

void AP_ReconRepere::match(
		float x,
		float y,
		float yaw,
		float rdetect,
		const vector<GPoint2D>& pylones,
		const vector<GPoint2D>& _reperes,
		vector<bool>& ident_,
		vector<GPoint2D>& pylid_,
		vector<GPoint2D>& repid_) {
	vector<GPoint2D> carte;
	float cos_yaw, sin_yaw;
	if (AP_DEBUG) cout << "void AP_ReconRepere::match" << endl;
	cos_yaw = cos(yaw);
	sin_yaw = sin(yaw);
	carte.clear();
	// création de la carte
	for (unsigned int i = 0; i < pylones.size(); i++) {
		carte.push_back(GPoint2D(x + cos_yaw * (pylones[i].x) - sin_yaw
				* (pylones[i].y), y + sin_yaw * (pylones[i].x) + cos_yaw
				* (pylones[i].y)));
		if (AP_DEBUG) cout << "carte " << i << "(" << carte[i].x << ","
				<< carte[i].y << ")" << endl;
	}
	pylid_.clear();
	repid_.clear();
	// pour chaque repère on balaie la carte à la recherche du plus proche
	for (int k = 0; k < _reperes.size(); k++) {
		float distance, a, b, c;
		int index; // index du poteau qui match
		ident_[k] = false;
		c = rdetect; // rayon de détection
		if (AP_DEBUG) cout << "repere " << k << "(" << _reperes[k].x << ","
				<< _reperes[k].y << ")" << endl;
		for (unsigned int i = 0; i < pylones.size(); i++) {
			a = carte[i].x - _reperes[k].x;
			a *= a;
			b = carte[i].y - _reperes[k].y;
			b *= b;
			distance = sqrt(a + b); // distance au point auquel on attendait le poteau
			if (distance < c) {
				c = distance;
				index = i;
				ident_[k] = true;
			}
		}
		if (ident_[k]) {
			pylid_.push_back(pylones[index]);
			repid_.push_back(_reperes[k]);
			if (AP_DEBUG) cout << "point carte (" << carte[index].x << ","
					<< carte[index].y << ")" << " match avec " << "("
					<< _reperes[k].x << "," << _reperes[k].y << ")" << endl;
		}
	}
	if (AP_DEBUG) cout << "Nombre de pylones reconnus" << pylid_.size() << endl;
}

void AP_ReconRepere::match(
		float x,
		float y,
		float yaw,
		float rdetect,
		const vector<GPoint2D>& pylones) {
	if (AP_DEBUG) cout << "void AP_ReconRepere::match" << endl;
	match(x, y, yaw, rdetect, pylones, reperes, ident, pylid, repid);
}

/*
 * Transforme le vecteur _pylones à n composantes en une matrice 2n x 4
 */

short int AP_Estimer::pyltoarray(
		vector<GPoint2D> _pylones,
		real_2d_array& out,
		ae_int_t& m,
		ae_int_t& n)// transforme un vecteur "pylone" en matrice
{
	short int rep = 0;
	m = 2 * _pylones.size();
	n = 4;
	out.setlength(m, n);
	for (unsigned short int i = 0; i < _pylones.size(); i++) {
		unsigned short int j = i + _pylones.size();
		out[i][0] = _pylones[i].x;
		out[i][1] = -_pylones[i].y;
		out[i][2] = 1;
		out[i][3] = 0;
		out[j][0] = _pylones[i].y;
		out[j][1] = _pylones[i].x;
		out[j][2] = 0;
		out[j][3] = 1;
		rep++;
	}
	return rep;
}

short int AP_Estimer::reptoarray(vector<GPoint2D> _repid, real_2d_array& out) {
	short int rep = 0;
	short int m = 2 * _repid.size();
	out.setlength(m, 1);
	for (unsigned short int i = 0; i < _repid.size(); i++) {
		unsigned short int j = i + _repid.size();
		out[i][0] = _repid[i].x;
		out[j][0] = _repid[i].y;
		rep++;
	}
	return rep;
}

bool AP_Estimer::pinv(
		ae_int_t m,
		ae_int_t n,
		real_2d_array& in,
		real_2d_array& out,
		double tolerance) //pseudo inverse
{
	ae_int_t uneeded = 2;
	ae_int_t vtneeded = 2;
	ae_int_t additionalmemory = 2;
	real_2d_array u, vt; //vecteurs singuliers gauche,droite;
	real_1d_array w; // valeurs singulières
	real_2d_array interm;
	real_2d_array wpinv;// matrice inverse des val sing.
	//bool exit;
	if (rmatrixsvd(in, m, n, uneeded, vtneeded, additionalmemory, w, u, vt)) {
		//inversion de w
		wpinv.setlength(n, m);
		for (ae_int_t i = 0; i < n; i++) {
			for (ae_int_t j = 0; j < m; j++) {
				wpinv[i][j] = (i == j)
						? (w[i] > (tolerance) ? 1 / w[i] : 0)
						: 0;
			}
		}

		// calcul de v*w+dans interm
		interm.setlength(n, m);// dimensions de interm et de A+
		rmatrixgemm(n, m, n, 1, vt, 0, 0, 1, wpinv, 0, 0, 0, 0, interm, 0, 0); // interm stocke bien v*wpinv

		//calcul complet dans out
		out.setlength(n, m);
		rmatrixgemm(n, m, m, 1, interm, 0, 0, 0, u, 0, 0, 1, 0, out, 0, 0); //out stocke interm*u^T
	}
	return true;//en attendant implémentation avec try catch
}

GPoint3D AP_Estimer::arraytopose(real_2d_array& in) // transforme le vecteur position alglib en x,y,yaw
{
	GPoint3D rep;
	rep.x = (float) in[2][0];
	rep.y = (float) in[3][0];
	rep.z = (float) atan2(in[1][0], in[0][0]);
	return rep;
}

bool AP_Estimer::estimer_pose(
		const vector<GPoint2D>& pylid,
		const vector<GPoint2D>& repid,
		float& x,
		float xp,
		float& y,
		float yp,
		float& vx,
		float& vy,
		float yaw,
		float& yaw_offset,
		long int dtms) {
	float cos_yaw, sin_yaw;
	GPoint3D posm;
	if (AP_DEBUG) cout << "bool AP_Estimer::estimer_pose" << endl;
	switch (pylid.size()) {
	case 0:
		return false;
		break;
	case 1:
		cos_yaw = cos(yaw);
		sin_yaw = sin(yaw);
		x = (repid[0].x - cos_yaw * pylid[0].x + sin_yaw * pylid[0].y);
		y = (repid[0].y - sin_yaw * pylid[0].x - cos_yaw * pylid[0].y);
		vx = (1000.0 / dtms) * (x - xp);
		vy = (1000.0 / dtms) * (y - yp);
		return true;
		break;
	default:
		real_2d_array pylarray; // matrice 4 colonnes représentant les poteaux vus
		real_2d_array inv4; // future pinv de pylarray
		real_2d_array rarray; // matrice colonne repères
		real_2d_array array4; // matrice colonne position
		ae_int_t m, n;
		pyltoarray(pylid, pylarray, m, n);
		pinv(m, n, pylarray, inv4, 2 >> 30);
		reptoarray(repid, rarray);
		array4.setlength(4, 1);
		rmatrixgemm(n, 1, m, 1, inv4, 0, 0, 0, rarray, 0, 0, 0, 0, array4, 0, 0);
		posm = arraytopose(array4);
		x = posm.x;
		y = posm.y;
		vx = (1000.0 / dtms) * (x - xp);
		vy = (1000.0 / dtms) * (y - yp);
		//		yaw_offset += (yaw - posm.z);// corrige une partie du lacet
		return true;
		break;
	}
}

bool AP_Estimer::estimer_pose(const vector<GPoint2D>& pylid, const vector<
		GPoint2D>& repid, float yaw, float& yaw_offset, long int dtms) {
	estim_val = estimer_pose(
			pylid,
			repid,
			pose.x,
			pose.x,
			pose.y,
			pose.y,
			vit.x,
			vit.y,
			yaw,
			yaw_offset,
			dtms);
	return estim_val;
}

void AP_Estimateur::prediction(
		GPose3D pose_p,
		GPoint3D v_p,
		long int dtms,
		GPose3D& pose_s,
		GPoint3D& v_s) {
	float ax, ay;
	float cos_psi, sin_psi, tan_phi, cos_theta, tan_theta;
	float dt = dtms / 1000.0;
	cos_psi = cos(pose_p.yaw());
	sin_psi = sin(pose_p.yaw());
	tan_phi = tan(pose_p.roll());
	cos_theta = cos(pose_p.pitch());
	tan_theta = tan(pose_p.pitch());
	if (cos_theta != 0) {
		ax = cos_psi * tan_theta + sin_psi * (tan_phi / cos_theta);
		ay = sin_psi * tan_theta - cos_psi * (tan_phi / cos_theta);
		ax *= SIMU_MRPT ? 10 : 9.81;
		ay *= SIMU_MRPT ? 10 : 9.81;
	} else {
		ax = 0;
		ay = 0;
		// (c'est impossible)
	}
	pose_s.x(pose_p.m_x + dt * v_p.x + .5 * dt * dt * ax);
	pose_s.y(pose_p.m_y + dt * v_p.y + .5 * dt * dt * ay);
	v_s.x = v_p.x + dt * ax;
	v_s.y = v_p.y + dt * ay;
}

void AP_Estimateur::estimateur(
		GPose3D pose_m,
		GPoint3D v_m,
		GPose3D pose_p,
		GPoint3D v_p,
		long int dtms,
		float kx,
		float kv,
		GPose3D& pose_e_,
		GPoint3D& v_e_) {
	GPose3D pose_pred;
	GPoint3D v_pred;
	prediction(pose_p, v_p, dtms, pose_pred, v_pred);
	pose_e_.x(pose_pred.x() + kx * (pose_m.x() - pose_pred.x()));
	pose_e_.y(pose_pred.y() + kx * (pose_m.y() - pose_pred.y()));
	v_e_.x = v_pred.x + kv * (v_m.x - v_pred.x);
	v_e_.y = v_pred.y + kv * (v_m.y - v_pred.y);
}

void AP_Estimateur::estimateur(
		GPose3D pose_m,
		GPoint3D v_m,
		long int dtms,
		float kx,
		float kv) {
	GPose3D pose_p = pose_e;
	GPoint3D v_p = v_e;
	estimateur(pose_m, v_m, pose_p, v_p, dtms, kx, kv, pose_e, v_e);
	pose_e.setYawPitchRoll(pose_m.yaw(), pose_m.pitch(), pose_m.roll());
	pose_e.z(pose_m.z());
}

AP_Estimateur::AP_Estimateur() {
	pose_e = GPose3D(0, 0, 0, 0, 0, 0);
	v_e = GPoint3D(0.0, 0.0, 0.0);
	kx = 0.455855;
	kv = 0.54505;
}

AP_LocalisationWrapper::AP_LocalisationWrapper() {
	if (AP_DEBUG) cout << "AP_LocalisationWrapper::AP_LocalisationWrapper"
			<< endl;
	pose_m = GPose3D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // position mesurée en cartésiennes sur la carte (x,y,z,yaw,pitch,roll)
	v_m = GPoint3D(0.0, 0.0, 0.0); // vitesse mesurée
	yaw_offset = 0.0;
}

void AP_LocalisationWrapper::lireHYawPitchRoll(
		float h,
		float yaw,
		float pitch,
		float roll) {
	pose_m.setYawPitchRoll(yaw - yaw_offset, pitch, roll);
	pose_m.z(h);
}
/*
 * loc: à executer à condition d'avoir déja au moins un repère de rentré
 */

bool AP_LocalisationWrapper::loc(
		const TypeScan _scan,
		int nbpointsnominal,
		float seuil,
		float pitch,
		float roll,
		float yaw,
		float pose_z,
		float diametre,
		float rcontig,
		float rdetect) {
	bool rep = false;
	if (AP_DEBUG) cout << "bool AP_LocalisationWrapper::loc" << endl;
	// recopie des données entrées:
	lireHYawPitchRoll(pose_z, yaw, pitch, roll);

	// on ne met à jour x et y que si le scan est exploitable
	if (analysescan.analyserScan(
			_scan,
			seuil,
			nbpointsnominal,
			roll,
			pitch,
			diametre,
			rcontig)) {
		// les positions envoyées dans le match ont intérêt à être les plus proches possibles de celles à estimer
		// ici on renvoie la position précédente pour faire le match
		reconrepere.match(
				pose_m.x(),
				pose_m.y(),
				pose_m.yaw(),
				rdetect,
				analysescan.pylones);
		// une fois le match fait, on l'utilise pour mettre à jour l'estimation et yaw_offset
		rep = estimer.estimer_pose(
				reconrepere.pylid,
				reconrepere.repid,
				pose_m.yaw(),
				yaw_offset,
				analysescan.dt());
		if (rep) {
			// si l'estimation est valable, on met à jour la position complète et la vitesse
			pose_m.x(estimer.pose.x);
			pose_m.y(estimer.pose.y);
			v_m.x = estimer.vit.x;
			v_m.y = estimer.vit.y;
			if (SPIT_POSE_M) cout << "m[x,y,vx,vy]=" << pose_m.x() << "\t"
					<< pose_m.y() << "\t" << v_m.x << "\t" << v_m.y << endl;
			// on filtre tout ça
			estimateur.estimateur(
					pose_m,
					v_m,
					analysescan.dt(),
					estimateur.kx,
					estimateur.kv);
		}
	}
	return rep;
}

AP_PiloteHuit::AP_PiloteHuit() {
	etape = 0;
	cur_wpt = 0;
	//generateHuitWpts();
	generateBonjourPoteau();// WPT A/R amplitude 2m40
	vxZincr = 0;
	vyZincr = 0;
}

AP_PiloteHuit::~AP_PiloteHuit() {
	waypts.clear();
}
inline void AP_PiloteHuit::ligneDroite(
		GPose3D pose_e,
		GPoint3D v_e,
		GPoint2D depart,
		GPoint2D destination,
		float v_consigne,
		float K_r,
		float K_a,
		float K_m,
		float& a_cx,
		float& a_cy,
		float& a_consigne,
		float& gamma) {
	float cos_gamma, sin_gamma, d, v_aux;
	float a_rx, a_ry, a_ax, a_ay, a_mx, a_my;
	// on calcule l'accélération de consigne
	gamma = atan2(destination.y - depart.y, destination.x - depart.x); // direction de déplacement
	cos_gamma = cos(gamma);
	sin_gamma = sin(gamma);
	d = cos_gamma * (pose_e.y() - depart.y) - sin_gamma * (pose_e.x()
			- depart.x); // distance à la trajectoire
	// calcul du rappel
	a_rx = K_r * d * sin_gamma;
	a_ry = -K_r * d * cos_gamma;
	// calcul de l'amorti
	v_aux = -v_e.x * sin_gamma + v_e.y * cos_gamma; //projeté de la vitesse sur l'accélération de rappel
	a_ax = -K_a * (v_aux * (-sin_gamma));
	a_ay = -K_a * (v_aux * cos_gamma);
	// calcul de l'accélération motrice
	v_aux = v_e.x * cos_gamma + v_e.y * sin_gamma; //projeté de la vitesse du drone sur la trajectoire consigne
	a_mx = K_m * (v_consigne - v_aux) * cos_gamma;
	a_my = K_m * (v_consigne - v_aux) * sin_gamma;
	// calcul de la consigne totale
	a_cx = a_rx + a_ax + a_mx;
	a_cy = a_ry + a_ay + a_my;
	a_consigne = sqrt((a_cx) * (a_cx) + (a_cy) * (a_cy));
}

inline void AP_PiloteHuit::virage(
		GPose3D pose_e,
		GPoint3D v_e,
		GPoint2D centre,
		float rvirage,
		float v_consigne,
		enum_sens sens,
		float K_r,
		float K_a,
		float K_m,
		float& a_cx,
		float& a_cy,
		float& a_consigne,
		float& delta) {
	float cos_delta, sin_delta, d, v_aux, a_aux;
	float a_rx, a_ry, a_ax, a_ay, a_mx, a_my, a_cpx, a_cpy;
	float a, b;
	// on calcule l'accel de consigne
	delta = atan2(pose_e.y() - centre.y, pose_e.x() - centre.x); // e_r
	cos_delta = cos(delta);
	sin_delta = sin(delta);
	a = (pose_e.y() - centre.y);
	a *= a;
	b = (pose_e.x() - centre.x);
	b *= b;
	d = sqrt(a + b); // distance à la trajectoire
	// calcul du rappel
	a_rx = K_r * (rvirage - d) * cos_delta;
	a_ry = K_r * (rvirage - d) * sin_delta;
	// calcul de l'amorti
	v_aux = v_e.x * cos_delta + v_e.y * sin_delta; //vitesse radiale
	a_ax = -K_a * v_aux * cos_delta;
	a_ay = -K_a * v_aux * sin_delta;
	// calcul de la motrice
	v_aux = -v_e.x * sin_delta + v_e.y * cos_delta;// vitesse orthoradiale algébrique
	a_aux = K_m * (sens * v_consigne - v_aux);
	a_mx = a_aux * (-sin_delta);//tient compte du sens
	a_my = a_aux * (cos_delta);//tient compte du sens
	// calcul de la centripète
	if (d != 0) {
		a_cpx = -v_aux * v_aux * cos_delta / d;
		a_cpy = -v_aux * v_aux * sin_delta / d;
	} else {
		a_cpx = a_cpy = 0;
	}
	// consigne totale
	a_cx = a_rx + a_ax + a_mx + a_cpx;
	a_cy = a_ry + a_ay + a_my + a_cpy;
	a_consigne = sqrt((a_cx) * (a_cx) + (a_cy) * (a_cy));
}

void AP_PiloteHuit::calculAccelC(
		GPose3D pose_e,
		GPoint3D v_e,
		int etape,
		float rvirage,
		float midistance,
		float vlent,
		float vrapide,
		float Kr,
		float Km,
		float& a_cx_,
		float& a_cy_,
		float& a_consigne_,
		float& psi_c_) {
	float gamma, delta, temp;
	float Ka = 2.2 * sqrt(Kr);
	float marge = (1.5) * (vrapide + vlent) / Km;
	switch (etape) {
	case 0:// décollage
		a_cx_ = -Ka * (v_e.x);
		a_cy_ = -Ka * (v_e.y);
		psi_c_ = -M_PI / 2;
		break;
	case 1:// ligne droite
		ligneDroite(
				pose_e,
				v_e,
				GPoint2D(-midistance, -rvirage),
				GPoint2D(midistance, rvirage),
				(pose_e.x() > midistance - marge) ? vlent : vrapide,
				Kr,
				Ka,
				Km,
				a_cx_,
				a_cy_,
				a_consigne_,
				gamma);
		if (pose_e.x() > -midistance / 10.0) {
			psi_c_ = gamma - 80 * M_PI / 180.0;
		} else {
			psi_c_ = gamma + 80 * M_PI / 180.0;
		}
		break;
	case 2:// virage
		virage(
				pose_e,
				v_e,
				GPoint2D(midistance, 0),
				rvirage,
				vlent,
				SENS_INDIRECT,
				Kr,
				Ka,
				Km,
				a_cx_,
				a_cy_,
				a_consigne_,
				delta);
		psi_c_ = delta - M_PI - M_PI / 6;
		//psi_c=pose_e.yaw()+gisement;
		break;
	case 3:// ligne droite
		ligneDroite(
				pose_e,
				v_e,
				GPoint2D(midistance, -rvirage),
				GPoint2D(-midistance, rvirage),
				(pose_e.x() < -midistance + marge) ? vlent : vrapide,
				Kr,
				Ka,
				Km,
				a_cx_,
				a_cy_,
				a_consigne_,
				gamma);
		if (pose_e.x() < midistance / 10.0) {
			psi_c_ = gamma + 80 * M_PI / 180.0;
		} else {
			psi_c_ = gamma - 80 * M_PI / 180.0;
		}
		break;
	case 4://virage
		virage(
				pose_e,
				v_e,
				GPoint2D(-midistance, 0),
				rvirage,
				vlent,
				SENS_DIRECT,
				Kr,
				Ka,
				Km,
				a_cx_,
				a_cy_,
				a_consigne_,
				delta);
		/*
		 if (pose_e.y() > 0) {
		 psi_c = delta - M_PI + M_PI / 6;
		 } else {
		 psi_c = delta + M_PI + M_PI / 6;
		 }
		 //*/
		psi_c = delta - M_PI + M_PI / 6;
		//psi_c=pose_e.yaw()+gisement;
		break;
	}
}

int AP_PiloteHuit::etapeSuivte(
		int etape,
		float rvirage,
		float midistance,
		GPose3D pose_e) {
	if (AP_DEBUG) cout << "hauteur" << pose_e.z() << endl;
	switch (etape) {
	case 0:
		return (-pose_e.z() > 1) ? 1 : 0;
	case 1:
		return (pose_e.x() > midistance - (midistance * rvirage * rvirage)
				/ (midistance * midistance + rvirage * rvirage)) ? 2 : 1;
	case 2:
		return ((pose_e.x() < midistance - (midistance * rvirage * rvirage)
				/ (midistance * midistance + rvirage * rvirage)) && (pose_e.y()
				< 0)) ? 3 : 2;
	case 3:
		return (-pose_e.x() > midistance - (midistance * rvirage * rvirage)
				/ (midistance * midistance + rvirage * rvirage)) ? 4 : 3;
	case 4:
		return ((-pose_e.x() < midistance - (midistance * rvirage * rvirage)
				/ (midistance * midistance + rvirage * rvirage)) && (pose_e.y()
				< 0)) ? 1 : 4;
	}
}

void AP_PiloteHuit::calculYawPitchRollC(
		float _a_cx,
		float _a_cy,
		float _a_consigne,
		float _xi_sat,
		float& _theta_c,
		float& _phi_c,
		float& _psi_c,
		float yaw,
		float yaw_offset) {
	float alpha, xi, xi_c;
	if (AP_DEBUG) cout << "calculYawPitchRoll: " << "(acx,acy,_a_consigne)="
			<< _a_cx << " " << _a_cy << " " << _a_consigne << endl;
	if (AP_DEBUG) cout << "calculYawPitchRoll: " << "xisat=" << _xi_sat << endl;
	xi_c = atan(_a_consigne / 9.81);
	xi = sat(xi_c, _xi_sat); // saturation;
	alpha = atan2(_a_cy, _a_cx);
	_theta_c = sat(-atan(tan(xi) * cos(alpha - yaw)), _xi_sat);
	_phi_c = sat(asin(sin(xi) * sin(alpha - yaw)), _xi_sat);
	_psi_c += yaw_offset;
}

void AP_PiloteHuit::piloteHuit(
		GPose3D pose_e,
		GPoint3D v_e,
		float yaw,
		float yaw_offset,
		float rvirage,
		float midistance,
		float Kr,
		float Km,
		float vrapide,
		float vlent,
		float xi_sat) {
	float a_cx, a_cy, a_consigne;
	etape = etapeSuivte(etape, rvirage, midistance, pose_e);
	calculAccelC(
			pose_e,
			v_e,
			etape,
			rvirage,
			midistance,
			vlent,
			vrapide,
			Kr,
			Km,
			a_cx,
			a_cy,
			a_consigne,
			psi_c);
	calculYawPitchRollC(
			a_cx,
			a_cy,
			a_consigne,
			xi_sat,
			theta_c,
			phi_c,
			psi_c,
			yaw,
			yaw_offset);
	if (AP_DEBUG) cout << "etape " << etape << endl;
}

void AP_PiloteHuit::piloteZero(
		GPose3D pose_m,
		GPoint3D v_m,
		float yaw_offset,
		float kp,
		float ki,
		float kd,
		float e_int_sat,
		float s_int_sat,
		float dt,
		float ang_max_deg) {
	//psi_c = 0;
	float cos_yaw, sin_yaw;
	GPoint3D pos(pose_m.m_x, pose_m.m_y, 0);
	GPoint3D next((float) 0.0, (float) 0.0, 2);
	GPoint3D dir = next - pos;
	GPoint3D dirsat;
	if (dirsat.x > e_int_sat) dirsat.x = e_int_sat;
	if (dirsat.x < -e_int_sat) dirsat.x = -e_int_sat;
	if (dirsat.y > e_int_sat) dirsat.y = e_int_sat;
	if (dirsat.y < -e_int_sat) dirsat.y = -e_int_sat;
	cos_yaw = cos(pose_m.yaw());
	sin_yaw = sin(pose_m.yaw());
	float vx = kd * v_m.x - kp * dir.x;
	float vy = -kd * v_m.y + kp * dir.y;
	vxZincr += -ki * dt * dirsat.x;
	vyZincr += +ki * dt * dirsat.y;
	if (vxZincr > s_int_sat) vxZincr = s_int_sat;
	if (vxZincr < -s_int_sat) vxZincr = -s_int_sat;
	if (vyZincr > s_int_sat) vyZincr = s_int_sat;
	if (vyZincr < -s_int_sat) vyZincr = -s_int_sat;
	vx += vxZincr;
	vy += vyZincr;
	theta_c = vx * cos_yaw - vy * sin_yaw;
	phi_c = vx * sin_yaw + vy * cos_yaw;
	if (phi_c > ang_max_deg * M_PI / 180) phi_c = ang_max_deg * M_PI / 180;
	if (phi_c < -ang_max_deg * M_PI / 180) phi_c = -ang_max_deg * M_PI / 180;
	if (theta_c > ang_max_deg * M_PI / 180) theta_c = ang_max_deg * M_PI / 180;
	if (theta_c < -ang_max_deg * M_PI / 180) theta_c = -ang_max_deg * M_PI
			/ 180;
	psi_c = yaw_offset - M_PI / 2;
}

void AP_PiloteHuit::piloteWpt(
		GPose3D pose_m,
		GPoint3D v_m,
		float yaw_offset,
		float kp,
		float ki,
		float kd,
		float e_int_sat,
		float s_int_sat,
		float dt,
		float ang_max_deg) {
	float cos_yaw = cos(pose_m.yaw()), sin_yaw = sin(pose_m.yaw());
	psi_c = waypts[cur_wpt].z;

	GPoint3D pos(pose_m.m_x, pose_m.m_y, 0);
	GPoint3D next(waypts[cur_wpt].x, waypts[cur_wpt].y, 0);
	GPoint3D dir = next - pos;
	GPoint3D dirsat=dir;
	if (dirsat.x > e_int_sat) dirsat.x = e_int_sat;
	if (dirsat.x < -e_int_sat) dirsat.x = -e_int_sat;
	if (dirsat.y > e_int_sat) dirsat.y = e_int_sat;
	if (dirsat.y < -e_int_sat) dirsat.y = -e_int_sat;

	if (dir.normxy() < 0.5) {
		cout << "cur_wpt passe de " << cur_wpt << " à " ;
		cur_wpt++;
		if (cur_wpt == waypts.size()) {
			cur_wpt = 0;
		}
		cout << cur_wpt << endl;
		return;
	}

	//float vec_rot = atan2(dir.y, dir.x) - pose_m.yaw();

	float vx = kd * v_m.x - kp * dirsat.x;
	float vy = -kd * v_m.y + kp * dirsat.y;
	vxZincr += -ki * dt * dirsat.x;
	vyZincr += +ki * dt * dirsat.y;
	if (vxZincr > s_int_sat) vxZincr = s_int_sat;
	if (vxZincr < -s_int_sat) vxZincr = -s_int_sat;
	if (vyZincr > s_int_sat) vyZincr = s_int_sat;
	if (vyZincr < -s_int_sat) vyZincr = -s_int_sat;
	vx += vxZincr;
	vy += vyZincr;
	theta_c = vx * cos_yaw - vy * sin_yaw;
	phi_c = vx * sin_yaw + vy * cos_yaw;
	if (phi_c > ang_max_deg * M_PI / 180) phi_c = ang_max_deg * M_PI / 180;
	if (phi_c < -ang_max_deg * M_PI / 180) phi_c = -ang_max_deg * M_PI / 180;
	if (theta_c > ang_max_deg * M_PI / 180) theta_c = ang_max_deg * M_PI / 180;
	if (theta_c < -ang_max_deg * M_PI / 180) theta_c = -ang_max_deg * M_PI
			/ 180;
	psi_c = yaw_offset - M_PI / 2;
}

void AP_PiloteHuit::piloteWpt2(
		GPose3D pose_m,
		GPoint3D v_m,
		float yaw_offset,
		float kp,
		float ki,
		float kd,
		float e_int_sat,
		float s_int_sat,
		float dt,
		float ang_max_deg) {
	float cos_yaw = cos(pose_m.yaw()), sin_yaw = sin(pose_m.yaw());
	psi_c = waypts[cur_wpt].z;

	GPoint3D pos(pose_m.m_x, pose_m.m_y, 0);
	GPoint3D next(waypts[cur_wpt].x, waypts[cur_wpt].y, 0);
	GPoint3D dir = next - pos;
	GPoint3D dirsat=sat(dir,e_int_sat);

	if (dir.normxy() < 0.5) {
		cout << "cur_wpt passe de " << cur_wpt << " à " ;
		cur_wpt++;
		if (cur_wpt == waypts.size()) {
			cur_wpt = 0;
		}
		cout << cur_wpt << endl;
		return;
	}

	//float vec_rot = atan2(dir.y, dir.x) - pose_m.yaw();
	float a_cpx = kp * dirsat.x;
	float a_cpy = kp * dirsat.y;
	float a_cdx = -kd * v_m.x;
	float a_cdy = -kd * v_m.y;
	vxZincr += ki * dt * dirsat.x;
	vyZincr += ki * dt * dirsat.y;
	float a_cix = vxZincr = sat(vxZincr,s_int_sat);
	float a_ciy = vyZincr = sat(vyZincr,s_int_sat);
	float a_cx = a_cpx + a_cdx + a_cix;
	float a_cy = a_cpy + a_cdy + a_ciy;
	float a_consigne = sqrt(a_cx*a_cx + a_cy*a_cy);

	calculYawPitchRollC(
			a_cx,
			a_cy,
			a_consigne,
			ang_max_deg * M_PI / 180.0,
			theta_c,
			phi_c,
			psi_c,
			pose_m.yaw(),
			yaw_offset);
}


void AP_PiloteHuit::generateHuitWpts() {
	waypts.clear();
	waypts.push_back(GPoint3D(3, 1, -45 * M_PI / 180));
	waypts.push_back(GPoint3D(5, 1.5, -100 * M_PI / 180));
	waypts.push_back(GPoint3D(6.5, 0.5, -135 * M_PI / 180));
	waypts.push_back(GPoint3D(6.5, -0.5, -180 * M_PI / 180));
	waypts.push_back(GPoint3D(5, -1.5, -230 * M_PI / 180));
	waypts.push_back(GPoint3D(3, -1, -260 * M_PI / 180));
	waypts.push_back(GPoint3D(0, 0, -290 * M_PI / 180));
	waypts.push_back(GPoint3D(-3, 1, -200 * M_PI / 180));
	waypts.push_back(GPoint3D(-5, 1.5, -120 * M_PI / 180));
	waypts.push_back(GPoint3D(-6.5, 0.5, -60 * M_PI / 180));
	waypts.push_back(GPoint3D(-6.5, -0.5, 0 * M_PI / 180));
	waypts.push_back(GPoint3D(-5, -1.5, 60 * M_PI / 180));
	waypts.push_back(GPoint3D(-3, -1, 120 * M_PI / 180));
	waypts.push_back(GPoint3D(0, 0, 60 * M_PI / 180));
}

void AP_PiloteHuit::generateBonjourPoteau() {
	waypts.clear();
	waypts.push_back(GPoint3D(-5.20, .5, -M_PI / 2));
	waypts.push_back(GPoint3D(0, 0, -M_PI / 2));
	waypts.push_back(GPoint3D(5.20, .5, -M_PI / 2));
	waypts.push_back(GPoint3D(0, 0, -M_PI / 2));
}
